#ifndef FEATURE_MATCH
#define FEATURE_MATCH
 
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <stdlib.h>
#include <sys/shm.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "opencv2/opencv.hpp"
// #include "opencv2/xfeatures2d.hpp"
#include <iostream>
#include <vector>
#include <string>
#include<pcl/io/pcd_io.h>
#include<pcl/point_types.h>
#include <tf/transform_broadcaster.h>
using namespace std;
using namespace cv;
using PointType = pcl::PointXYZI;
 
class FeatureMatch{
    public:
        Mat target_img_;
    
    private:
        std::vector<cv::KeyPoint> target_key_points_, frame_key_points_;
        cv::Mat target_description_, frame_description_;
        cv::Ptr<cv::ORB> orb_ = cv::ORB::create(50, 1.2f);
        // cv::Ptr<cv::xfeatures2d::SURF> orb_ = cv::xfeatures2d::SURF::create();
        cv::Mat H_;
        Rect bbox_ = cv::Rect(290,205,147,125);

    public:
        FeatureMatch(){ 
            Mat target_img = imread("/media/st/Application/Ubuntu/dataset/tj/1019/img/1019/training/004000.jpg");
            target_img_ = cv::Mat(cv::Size(target_img.size().width, target_img.size().height), CV_8UC3,cv::Scalar(152,158,153));
            target_img = target_img(bbox_);
            target_img.copyTo(target_img_(bbox_));
            orb_->detect(target_img_, target_key_points_);
            orb_->compute(target_img_, target_key_points_, target_description_);

        }
          
        ~FeatureMatch() {
        }   

        double cal_yaw(Mat frame, Rect frame_bbox){
            Mat frame_crop(Size(frame.size().width,frame.size().height), CV_8UC3, cv::Scalar(120,123,108));
            frame(frame_bbox).copyTo(frame_crop(frame_bbox));
            orb_->detect(frame_crop, frame_key_points_);
            orb_->compute(frame_crop,frame_key_points_, frame_description_);
            std::vector<cv::DMatch> matches, goodMin, goodRansac;
            cv::BFMatcher matcher(cv::NORM_HAMMING);

            // flann::Index flannIndex(imageDesc1, flann::LshIndexParams(12, 20, 2), cvflann::FLANN_DIST_HAMMING);
            // vector<DMatch> GoodMatchePoints;
            // Mat macthIndex(imageDesc2.rows, 2, CV_32SC1), matchDistance(imageDesc2.rows, 2, CV_32FC1);
            // flannIndex.knnSearch(imageDesc2, macthIndex, matchDistance, 2, flann::SearchParams());

            try{
                matcher.match(target_description_, frame_description_, matches);
            }catch(const cv::Exception& e){
                cerr<<e.what()<<endl;
                return 181;
            }
            matchMin(matches, goodMin);
            ransac(goodMin, target_key_points_, frame_key_points_, goodRansac, H_);  //相对于第一张图片
            if (H_.empty())
                return 181;
            double angle = getAngle(H_);

            //debug
            Mat out3;
            cv::drawMatches(target_img_, target_key_points_, frame_crop, frame_key_points_, goodRansac, out3);
            cv::imshow("RANSAC筛选", out3);
            waitKey(5000);
            
            return -angle*M_PI/180;
        }
        
        void ransac(std::vector<cv::DMatch> matches, std::vector<cv::KeyPoint> queryKeyPoint,
                    std::vector<cv::KeyPoint> trainKeyPoint, std::vector<cv::DMatch> &matchesRansac, cv::Mat &H)
        {
            // 定义保存匹配点的坐标
            std::vector<cv::Point2f> srcPoints(matches.size()), dstPoints(matches.size());
            // 保存从关键点中提取到的匹配点对坐标
            for (size_t i = 0; i < matches.size(); ++i)
            {
                srcPoints[i] = queryKeyPoint[matches[i].queryIdx].pt;
                dstPoints[i] = trainKeyPoint[matches[i].trainIdx].pt;
            }

            // 匹配点对进行RANSAC过滤
            std::vector<int> inliersMask(srcPoints.size());
            H = cv::findHomography(srcPoints, dstPoints, cv::RANSAC, 5, inliersMask);
            // 手动保留RANSAC过滤后的匹配点对
            for (size_t i = 0; i < inliersMask.size(); ++i)
            {
                if (inliersMask[i])
                {
                    matchesRansac.push_back(matches[i]);
                }
            }
        }

        void matchMin(std::vector<cv::DMatch> matches, std::vector<cv::DMatch> &goodMatches)
        {
            double minDist = 10000, maxDist = 0;
            for (size_t i = 0; i < matches.size(); ++i)
            {
                double temp = matches[i].distance;
                if (temp < minDist)
                {
                    minDist = temp;
                }
            }
            for (size_t i = 0; i < matches.size(); ++i)
            {
                if (matches[i].distance <= cv::max(2 * minDist, 20.0))
                {
                    goodMatches.push_back(matches[i]);
                }
            }
        }
 
        double getAngle(cv::Mat H)
        {
            //    double x,y;
            //    x=cvmGet(H,0,2);y=cvmGet(H,1,2);
            //缩放的比例
            double Sx,Sy;
            Sx=(double)sqrt( (double)H.at<double>(0,0)*H.at<double>(0,0) + (double) H.at<double>(1,0)*H.at<double>(1,0) );
            Sy=(double)sqrt( (double)H.at<double>(1,0)*H.at<double>(1,0) + (double)H.at<double>(1,1)*H.at<double>(1,1) );
            std::cout<<"缩放系数： "<<Sx<<std::endl;
            //旋转的角度
            double angle = acos(H.at<double>(0,0)/Sx)/M_PI*180;
            if(H.at<double>(0,1)<0)
                angle = -angle;
            std::cout<<"角度： "<<angle<<std::endl;
            // cout<<"H "<<H<<endl;
            return angle;
        }
};

// class PcDataLoader{
//     public:
        
// }
#endif
